#ifndef MAT3_H_
#define MAT3_H_

#include "../common.h"

#include "Mat.h"
#include "Vec3.h"

namespace physicsworks {

namespace math {

using exception::EUndefMathOpt;

template<class T, class Tr = real_traits<T> > class Mat3 : public Mat<T, Tr> {
private:
	typedef Mat<T, Tr> Base;

public:

	using Mat<T, Tr>::operator=;
	using Mat<T, Tr>::column;

	Mat3<T, Tr>() : Mat<T, Tr>(3, 3) {}
	Mat3<T, Tr>(const Mat<T, Tr>& m) : Mat<T, Tr>(m) {
		Assert<EBadArg>(!DMATH || m.m() == 3 && m.n() == 3, "The provided matrix must be 3x3");
	}
	Mat3<T, Tr>(const Vec<T>& c0, const Vec<T>& c1, const Vec<T>& c2)
	 : Mat<T, Tr>(3, 3) { column(0, c0); column(1, c1); column(2, c2); }
	Mat3<T, Tr>(const T v[9]) : Mat<T, Tr>(3, 3, v) {}
	
	/**
	 * Returns the c-th column as Vec3<T>.
	 */
	Vec3<T> column(size_t c) const {
		Vec3<T> v;
		for (size_t i = 0; i < 3; ++i)
			v(i) = Base::operator()(i, c);
		return v;
	}
	
	Vec3<T> x() const { return Base::column(0); }
	Vec3<T> y() const { return Base::column(1); }
	Vec3<T> z() const { return Base::column(2); }

	/**
	 * TODO: Test the execution speed of this against the common way.
	 */
	T determinant() const {
		return x() * y().cross(z());
	}
	
	/**
	 * Inverts the matrix.
	 */
	void inverse() {
		*this = Mat3<T, Tr>::inverse(*this);
	}

	static Mat3<T, Tr> inverse(const Mat3<T, Tr>& m);
	static Mat3<T, Tr> Zero() {
		Mat3<T, Tr> m = Mat3<T, Tr>();
		m.arr = 0;
		return m;
	}
	static Mat3<T, Tr> Identity() {
		Mat3<T, Tr> m = Mat3<T, Tr>();
		m.arr = 0;
		m(0,0) = 1; m(1,1) = 1; m(2,2) = 1;
		return m;
	}
	
	static Mat3<T, Tr> ZERO;
	static Mat3<T, Tr> IDENTITY;
};

template<class T, class Tr> Mat3<T, Tr> Mat3<T, Tr>::ZERO = Mat3<T, Tr>::Zero();
template<class T, class Tr> Mat3<T, Tr> Mat3<T, Tr>::IDENTITY = Mat3<T, Tr>::Identity();

/**
 * Returns the inverse matrix of the supplied one.
 */
template<class T, class Tr> Mat3<T, Tr> Mat3<T, Tr>::inverse(const Mat3<T, Tr>& m)
{
	Mat3<T, Tr> r;
	
	T det = m.determinant();
	
	Assert<EUndefMathOpt>(!DMATH || det != 0, "The determinant of the matrix must be nonzero to be invertible (nonsingular)");
	
	r(0,0) = m(0,0)*m(1,1)-m(0,1)*m(1,0); r(0,1) = m(0,2)*m(2,1)-m(0,1)*m(2,2); r(0,2) = m(0,1)*m(1,2)-m(0,2)*m(1,1);
	r(1,0) = m(1,2)*m(2,0)-m(1,0)*m(2,2); r(1,1) = m(0,0)*m(2,2)-m(0,2)*m(2,0); r(1,2) = m(0,2)*m(1,0)-m(0,0)*m(1,2);
	r(2,0) = m(1,0)*m(2,1)-m(1,1)*m(1,0); r(2,1) = m(0,1)*m(2,0)-m(0,0)*m(2,1); r(2,2) = m(0,0)*m(1,1)-m(0,1)*m(1,0);

	return 1/det * r;
}

}

}

#endif /*MAT3_H_*/
